#include <iostream>
using namespace std;
#include <stdio.h>
#include <cmath>

#include <mex.h>
#include "mexFileIO.h"

#include <vector>
#include <algorithm>
#include <queue>
#include <limits>
#include <map>
using namespace std;
#include <mexFileIO.h>
#include <szMexUtility.h>
#include <szMexUtilityTemplate.h>
#include <szMyNeighborOp.h>
#include <szMiscOperations.h>
#include <szConvexHull2D.h>
#include <ContourEQW.h>
#include <szParticleF.h>
#include <TriangulationWithMemento.h>
#include <FragmentInfo.h>
//#include <TriangulationHelper.h>
#include <Tree.h>
#include <GraphFactory.h>
typedef CParticleF graphKey;

GraphFactory<graphKey>* GraphFactory<graphKey>::_instance = NULL;

bool
coLinear(CParticleF& a, CParticleF& b, CParticleF& c, float precision = 1.0e-3)
{
	float d = Distance2Line(a, c, b);
	return d < precision;
}

vector<CParticleF>
removeDegenerate(vector<CParticleF> points)
{
	while(true)
	{
		vector<CParticleF> result;
		bool bChanged = false;
		for(int i=0; i<points.size(); ++i)
		{
			int i0=(i-1+points.size()) % points.size();
			int i2=(i+1) % points.size();
			if(coLinear(points[i0], points[i], points[i2]))
			{
				bChanged = true;
			}
			else
			{
				result.push_back(points[i]);
			}
		}
		if(bChanged == false)
		{
			return result;
		}
		else
		{
			points = result;
		}
	}
}


void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{   
	printf("%s: This build was compiled at %s %s\n", "StraightMedialAxis", __DATE__, __TIME__);
	if (nrhs < 1 || nlhs < 0)
	{
		mexErrMsgTxt("Usage: [X Y] = StraightMedialAxis(P, [iter delta])");
		return;
	}
	//Points
	vector<CParticleF> points;
	const int* dimsP;
	{
		vector<float> P0;
		mxClassID classIdP;
		int ndimP;
		LoadData(P0, prhs[0], classIdP, ndimP, &dimsP);
		for(int i=0; i<dimsP[0]; ++i)
		{
			float x = GetData2(P0, i, 0, dimsP[0], dimsP[1], (float)0);
			float y = GetData2(P0, i, 1, dimsP[0], dimsP[1], (float)0);
			points.push_back(CParticleF(x, y));
		}
	}
	int maxIter = 10;
	if(nrhs >= 2)
	{
		mxClassID classMode;
		ReadScalar(maxIter,prhs[1],classMode);
	} 
	float delta = 1;
	if(nrhs >= 3)
	{
		mxClassID classMode;
		ReadScalar(delta,prhs[2],classMode);
	} 

	points = removeDegenerate(points);

	vector<CParticleF> v(points.size());
	for(int i=0; i<points.size(); ++i)
	{
		int i0=(i-1+points.size()) % points.size();
		int i2=(i+1) % points.size();
		float mx = (points[i0].m_X + points[i2].m_X)/2.0;
		float my = (points[i0].m_Y + points[i2].m_Y)/2.0;
		float vx = mx - points[i].m_X;
		float vy = my - points[i].m_Y;
		CParticleF m(mx, my);
		if(inside(m, points) == false)
		{
			vx -= vx;
			vy -= vy;
		}
		float len = sqrt(vx*vx + vy*vy);
		if(Abs(len) > 1.0e-6)
		{
			v[i].m_X = vx / len;
			v[i].m_Y = vy / len;
		}
		else
		{
			v[i].m_X = 0;
			v[i].m_Y = 0;
		}
	}

	vector<vector<CParticleF>> motion(maxIter+1);
	motion[0] = points;
	for(int i=1; i<=maxIter; ++i)
	{
		float t = i * delta;
		vector<CParticleF> points2(points.size());
		for(int j=0; j<points.size(); ++j)
		{
			float x = points[j].m_X + t * v[j].m_X;
			float y = points[j].m_Y + t * v[j].m_Y;
			points2[j].m_X = x;
			points2[j].m_Y = y;
		}
		motion[i] = points2;
	}

	if(nlhs >= 1)
	{
		const int dims[] = {points.size(), maxIter};
		vector<float> F(dims[0]*dims[1]);
		for(int i=0; i<dims[0]; ++i)
		{
			for(int j=0; j<dims[1]; ++j)
			{
				SetData2(F, i, j, dims[0], dims[1], motion[j][i].m_X);
			}
		}
		plhs[0] = StoreData(F, mxSINGLE_CLASS, 2, dims);
	}
	if(nlhs >= 2)
	{
		const int dims[] = {points.size(), maxIter};
		vector<float> F(dims[0]*dims[1]);
		for(int i=0; i<dims[0]; ++i)
		{
			for(int j=0; j<dims[1]; ++j)
			{
				SetData2(F, i, j, dims[0], dims[1], motion[j][i].m_Y);
			}
		}
		plhs[1] = StoreData(F, mxSINGLE_CLASS, 2, dims);
	}

	mexUnlock();
}

